#ifndef _MPU_MPU6050_H_
#define _MPU_MPU6050_H_
//#include "stm32f10x.h"

#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f10x.h"

/*
#define    TWI_Start()            //TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);   //清零TWINT、清零start状态、使能TWI.发出START 信号
#define    TWI_Wait()             while (!(TWCR & (1<<TWINT)));            //等待TWINT 置位， TWINT 置位表示START 信号已发出 ，TWDR数据稳定
#define    TWI_Status             (TWSR &0xF8)                            //检验TWI 状态寄存器，屏蔽预分频位
#define    TWI_Stop()             TWCR=(1<<TWINT)|(1<<TWEN)|(1<<TWSTO) ;    //发送STOP 信号
#define    TWI_SendAck()          (TWCR|=(1<<TWEA)) ;                       //使能TWI应答
#define    TWI_SendNoAck()        (TWCR&=~(1<<TWEA)) ;                      //不使能TWI应答
#define    TWI_RcvNckByte()       (TWCR=(1<<TWINT)|(1<<TWEN)) ;             //使能TWI不产生应答
#define    TWI_RcvAckByte()       (TWCR=(1<<TWINT)|(1<<TWEN)|(1<<TWEA));    //使能TWI、产生应答
#define    TWI_Writebyte(dat)     {TWDR=(dat);TWCR=(1<<TWINT)|(1<<TWEN);}


#define    TWI_ERR                 0
#define    TWI_CRR                 1
#define    TWI_WRITE                 0
#define    TWI_READ                 1
*/


#define MPU6050_SlaveAddress	0x68
#define MPU6050_RA_XG_OFFS_TC       	0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_YG_OFFS_TC       	0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_ZG_OFFS_TC       	0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_X_FINE_GAIN      	0x03 //[7:0] X_FINE_GAIN
#define MPU6050_RA_Y_FINE_GAIN      	0x04 //[7:0] Y_FINE_GAIN
#define MPU6050_RA_Z_FINE_GAIN      	0x05 //[7:0] Z_FINE_GAIN
#define MPU6050_RA_XA_OFFS_H        	0x06 //[15:0] XA_OFFS
#define MPU6050_RA_XA_OFFS_L_TC     	0x07
#define MPU6050_RA_YA_OFFS_H        	0x08 //[15:0] YA_OFFS
#define MPU6050_RA_YA_OFFS_L_TC     	0x09
#define MPU6050_RA_ZA_OFFS_H        	0x0A //[15:0] ZA_OFFS
#define MPU6050_RA_ZA_OFFS_L_TC     	0x0B
#define MPU6050_RA_XG_OFFS_USRH     	0x13 //[15:0] XG_OFFS_USR
#define MPU6050_RA_XG_OFFS_USRL     	0x14
#define MPU6050_RA_YG_OFFS_USRH     	0x15 //[15:0] YG_OFFS_USR
#define MPU6050_RA_YG_OFFS_USRL     	0x16
#define MPU6050_RA_ZG_OFFS_USRH     	0x17 //[15:0] ZG_OFFS_USR
#define MPU6050_RA_ZG_OFFS_USRL     	0x18



/*****************************************/
#define MPU6050_AUX_VDDIO			0X01//When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to 0, the auxiliary I2C bus high logic level is VLOGIC
#define MPU6050_aux_vddio					0X80
#define MPU6050_SMPLRT_DIV      	0x19//Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)

#define MPU6050_CONFIG          	0x1A//
#define MPU6050_DLPF_BW_188			0x01
#define MPU6050_DLPF_BW_98 			0x02
#define MPU6050_DLPF_BW_42 			0x03
#define MPU6050_DLPF_BW_20 			0x04
#define MPU6050_DLPF_BW_10 			0x05
#define MPU6050_DLPF_BW_5  			0x06
#define MPU6050_EXT_SYNC_DISABLE			0X00
#define MPU6050_EXT_SYNC_TEMP_OUT_L			0X08
#define MPU6050_EXT_SYNC_GYRO_XOUT_L		0X10
#define MPU6050_EXT_SYNC_GYRO_YOUT_L		0X18
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L		0X20
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L		0X28
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L		0X30
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L		0X38
#define MPU6050_DLPF_BW_256			0x00

#define MPU6050_GYRO_CONFIG     	0x1B
#define MPU6050_GYRO_RANGE_FS_250    	    0x00
#define MPU6050_GYRO_RANGE_FS_500    	    0x04
#define MPU6050_GYRO_RANGE_FS_1000   	    0x10
#define MPU6050_GYRO_RANGE_FS_2000   	    0x18
#define MPU6050_XG_ST  						0X20
#define MPU6050_YG_ST  						0X40
#define MPU6050_ZG_ST  						0X80


#define MPU6050_ACCEL_CONFIG    	0x1C
#define MPU6050_ACCEL_HPF_CUT_None			0x00
#define MPU6050_ACCEL_HPF_CUT_5Hz 			0x01
#define MPU6050_ACCEL_HPF_CUT_2_5Hz 		0x02
#define MPU6050_ACCEL_HPF_CUT_1_25Hz		0x03
#define MPU6050_ACCEL_HPF_CUT_0_63Hz 		0x04
#define MPU6050_ACCEL_FS_SEL_2g				0x00
#define MPU6050_ACCEL_FS_SEL_4g				0x08
#define MPU6050_ACCEL_FS_SEL_8g				0x10
#define MPU6050_ACCEL_FS_SEL_16g			0x18
#define MPU6050_XG_ST_BIT						0X80
#define MPU6050_YG_ST_BIT						0X40
#define MPU6050_ZG_ST_BIT						0X20


#define MPU6050_FF_THR          	0x1D
#define MPU6050_FF_DUR          	0x1E
#define MPU6050_MOT_THR         	0x1F
#define MPU6050_MOT_DUR         	0x20
#define MPU6050_ZRMOT_THR       	0x21
#define MPU6050_ZRMOT_DUR       	0x22


#define MPU6050_FIFO_EN         	0x23
#define MPU6050_FIFO_Disable				0x00
#define MPU6050_SLV_FIFO_EN_SLA0			0X01
#define MPU6050_SLV_FIFO_EN_SLA1			0X02
#define MPU6050_SLV_FIFO_ENSLA2				0X04
#define MPU6050_ACCEL_FIFO_EN				0X08
#define MPU6050_XG_FIFO_EN					0X10
#define MPU6050_YG_FIFO_EN					0X20
#define MPU6050_ZG_FIFO_EN					0X40
#define MPU6050_TEMP_FIFO_EN				0X80



#define MPU6050_I2C_MST_CTRL    	0x24
#define MPU6050_I2C_MST_CLK_348KHz			0X00
#define MPU6050_I2C_MST_CLK_333KHz			0X01
#define MPU6050_I2C_MST_CLK_320KHz			0X02
#define MPU6050_I2C_MST_CLK_308KHz			0X03
#define MPU6050_I2C_MST_CLK_296KHz			0X04
#define MPU6050_I2C_MST_CLK_286KHz			0X05
#define MPU6050_I2C_MST_CLK_276KHz			0X06
#define MPU6050_I2C_MST_CLK_267KHz			0X07
#define MPU6050_I2C_MST_CLK_258KHz			0X08
#define MPU6050_I2C_MST_CLK_500KHz			0X09
#define MPU6050_I2C_MST_CLK_471KHz			0X0A
#define MPU6050_I2C_MST_CLK_444KHz			0X0B
#define MPU6050_I2C_MST_CLK_421KHz			0X0C
#define MPU6050_I2C_MST_CLK_400KHz			0X0D
#define MPU6050_I2C_MST_CLK_381KHz			0X0E
#define MPU6050_I2C_MST_CLK_364KHz			0X0F
#define	MPU6050_I2C_MST_P_NSR				0X10
#define	MPU6050_SLA_3_FIFO_EN				0X20
#define	MPU6050_WAIT_FOR_ES					0X40
#define MPU6050_MULT_MST_EN					0X80


#define MPU6050_I2C_SLV0_ADDR   	0x25
#define MPU6050_I2C_SLV0_RW					0x80
#define MPU6050_I2C_SLV0_ADDR_bit6			0x40
#define MPU6050_I2C_SLV0_ADDR_bit5			0x20
#define MPU6050_I2C_SLV0_ADDR_bit4			0x10
#define MPU6050_I2C_SLV0_ADDR_bit3			0x08
#define MPU6050_I2C_SLV0_ADDR_bit2			0x04
#define MPU6050_I2C_SLV0_ADDR_bit1			0x02
#define MPU6050_I2C_SLV0_ADDR_bit0			0x01

#define MPU6050_I2C_SLV0_REG    	0x26
#define MPU6050_I2C_SLV0_CTRL   	0x27
#define MPU6050_I2C_SLV0_EN 				0X80
#define MPU6050_I2C_SLV0_BYTE_SW			0X40
#define MPU6050_I2C_SLV0_REG_DIS			0X20
#define MPU6050_I2C_SLV0_GRP				0X10
#define MPU6050_I2C_SLV0_LEN_BIT3			0X08
#define MPU6050_I2C_SLV0_LEN_BIT2			0X04
#define MPU6050_I2C_SLV0_LEN_BIT1			0X02
#define MPU6050_I2C_SLV0_LEN_BIT0			0X01


#define MPU6050_I2C_SLV1_ADDR   	0x28
#define MPU6050_I2C_SLV1_RW					0x80
#define MPU6050_I2C_SLV1_ADDR_bit6			0x40
#define MPU6050_I2C_SLV1_ADDR_bit5			0x20
#define MPU6050_I2C_SLV1_ADDR_bit4			0x10
#define MPU6050_I2C_SLV1_ADDR_bit3			0x08
#define MPU6050_I2C_SLV1_ADDR_bit2			0x04
#define MPU6050_I2C_SLV1_ADDR_bit1			0x02
#define MPU6050_I2C_SLV1_ADDR_bit0			0x01

#define MPU6050_I2C_SLV1_REG    	0x29
#define MPU6050_I2C_SLV1_CTRL   	0x2A
#define MPU6050_I2C_SLV1_EN 				0X80
#define MPU6050_I2C_SLV1_BYTE_SW			0X40
#define MPU6050_I2C_SLV1_REG_DIS			0X20
#define MPU6050_I2C_SLV1_GRP				0X10
#define MPU6050_I2C_SLV1_LEN_BIT3			0X08
#define MPU6050_I2C_SLV1_LEN_BIT2			0X04
#define MPU6050_I2C_SLV1_LEN_BIT1			0X02
#define MPU6050_I2C_SLV1_LEN_BIT0			0X01

#define MPU6050_I2C_SLV2_ADDR   	0x2B
#define MPU6050_I2C_SLV2_RW					0x80
#define MPU6050_I2C_SLV2_ADDR_bit6			0x40
#define MPU6050_I2C_SLV2_ADDR_bit5			0x20
#define MPU6050_I2C_SLV2_ADDR_bit4			0x10
#define MPU6050_I2C_SLV2_ADDR_bit3			0x08
#define MPU6050_I2C_SLV2_ADDR_bit2			0x04
#define MPU6050_I2C_SLV2_ADDR_bit1			0x02
#define MPU6050_I2C_SLV2_ADDR_bit0			0x01

#define MPU6050_I2C_SLV2_REG    	0x2C
#define MPU6050_I2C_SLV2_CTRL   	0x2D
#define MPU6050_I2C_SLV2_EN 				0X80
#define MPU6050_I2C_SLV2_BYTE_SW			0X40
#define MPU6050_I2C_SLV2_REG_DIS			0X20
#define MPU6050_I2C_SLV2_GRP				0X10
#define MPU6050_I2C_SLV2_LEN_BIT3			0X08
#define MPU6050_I2C_SLV2_LEN_BIT2			0X04
#define MPU6050_I2C_SLV2_LEN_BIT1			0X02
#define MPU6050_I2C_SLV2_LEN_BIT0			0X01


#define MPU6050_I2C_SLV3_ADDR   	0x2E
#define MPU6050_I2C_SLV3_RW					0x80
#define MPU6050_I2C_SLV3_ADDR_bit6			0x40
#define MPU6050_I2C_SLV3_ADDR_bit5			0x20
#define MPU6050_I2C_SLV3_ADDR_bit4			0x10
#define MPU6050_I2C_SLV3_ADDR_bit3			0x08
#define MPU6050_I2C_SLV3_ADDR_bit2			0x04
#define MPU6050_I2C_SLV3_ADDR_bit1			0x02
#define MPU6050_I2C_SLV3_ADDR_bit0			0x01

#define MPU6050_I2C_SLV3_REG    	0x2F
#define MPU6050_I2C_SLV3_CTRL   	0x30
#define MPU6050_I2C_SLV3_EN 				0X80
#define MPU6050_I2C_SLV3_BYTE_SW			0X40
#define MPU6050_I2C_SLV3_REG_DIS			0X20
#define MPU6050_I2C_SLV3_GRP				0X10
#define MPU6050_I2C_SLV3_LEN_BIT3			0X08
#define MPU6050_I2C_SLV3_LEN_BIT2			0X04
#define MPU6050_I2C_SLV3_LEN_BIT1			0X02
#define MPU6050_I2C_SLV3_LEN_BIT0			0X01

#define MPU6050_I2C_SLV4_ADDR   	0x31
#define MPU6050_I2C_SLV4_RW					0x80
#define MPU6050_I2C_SLV4_ADDR_bit6			0x40
#define MPU6050_I2C_SLV4_ADDR_bit5			0x20
#define MPU6050_I2C_SLV4_ADDR_bit4			0x10
#define MPU6050_I2C_SLV4_ADDR_bit3			0x08
#define MPU6050_I2C_SLV4_ADDR_bit2			0x04
#define MPU6050_I2C_SLV4_ADDR_bit1			0x02
#define MPU6050_I2C_SLV4_ADDR_bit0			0x01

#define MPU6050_I2C_SLV4_REG    	0x32
#define MPU6050_I2C_SLV4_DO     	0x33
#define MPU6050_I2C_SLV4_CTRL   	0x34
#define MPU6050_I2C_SLV4_EN 				0X80
#define MPU6050_I2C_SLV4_INT_EN				0X40
#define MPU6050_I2C_SLV4_REG_DIS			0X20
#define MPU6050_I2C_SLV4_MST_DLY_BIT4		0X10
#define MPU6050_I2C_SLV4_LEN_DLY_BIT3		0X08
#define MPU6050_I2C_SLV4_LEN_DLY_BIT2		0X04
#define MPU6050_I2C_SLV4_LEN_DLY_BIT1		0X02
#define MPU6050_I2C_SLV4_LEN_DLY_BIT0		0X01

#define MPU6050_I2C_SLV4_DI     	0x35
#define MPU6050_I2C_MST_STATUS  	0x36
#define MPU6050_PASS_THROUGH 				0X80
#define MPU6050_I2C_SLV4_DONE				0X40
#define MPU6050_I2C_LOST_ARB 				0X20
#define MPU6050_I2C_SLV4_NACK				0X10
#define MPU6050_I2C_SLV3_NACK				0X08
#define MPU6050_I2C_SLV2_NACK				0X04
#define MPU6050_I2C_SLV1_NACK				0X02
#define MPU6050_I2C_SLV0_NACK				0X01

#define MPU6050_INT_PIN_CFG     	0x37
#define MPU6050_INT_LEVEL 					0X80
#define MPU6050_INT_OPEB 					0X40
#define MPU6050_LATCH_INT_EN 				0X20
#define MPU6050_INT_CLEAR					0X10
#define MPU6050_FSYNC_INT_LEVEL				0X08
#define MPU6050_FSYNC_INT_EN				0X40
#define MPU6050_I2C_BYPASS_EN				0X20
#define MPU6050_CLKOUT_EN					0X10


#define MPU6050_INT_ENABLE      	0x38
#define MPU6050_FF_EN 						0X80
#define MPU6050_MOT_EN 						0X40
#define MPU6050_ZMOT_EN						0X20
#define MPU6050_FIFO_OFLOW_EN				0X10
#define MPU6050_I2C_MST_INT_EN				0X08
#define MPU6050_PLL_RDY_INT					0X04
#define MPU6050_DMP_INT							0x02
#define MPU6050_DATA_RDY_EN					0X01

#define MPU6050_DMP_INT_STATUS  	0x39
#define MPU6050_INT_STATUS      	0x3A
#define MPU6050_FF_INT 						0X80
#define MPU6050_MOT_INT						0X40
#define MPU6050_ZMOT_INT					0X20
#define MPU6050_FIFO_OFLOW_INT				0X10
#define MPU6050_I2C_MST_INT 				0X08
#define MPU6050_DATA_RDY_INT				0X01

#define MPU6050_ACCEL_XOUT_H    	0x3B
#define MPU6050_ACCEL_XOUT_L    	0x3C
#define MPU6050_ACCEL_YOUT_H    	0x3D
#define MPU6050_ACCEL_YOUT_L    	0x3E
#define MPU6050_ACCEL_ZOUT_H    	0x3F
#define MPU6050_ACCEL_ZOUT_L    	0x40
#define MPU6050_TEMP_OUT_H      	0x41
#define MPU6050_TEMP_OUT_L      	0x42
#define MPU6050_GYRO_XOUT_H     	0x43
#define MPU6050_GYRO_XOUT_L     	0x44
#define MPU6050_GYRO_YOUT_H     	0x45
#define MPU6050_GYRO_YOUT_L     	0x46
#define MPU6050_GYRO_ZOUT_H     	0x47
#define MPU6050_GYRO_ZOUT_L     	0x48
#define MPU6050_EXT_SENS_DATA_00	0x49
#define MPU6050_EXT_SENS_DATA_01	0x4A
#define MPU6050_EXT_SENS_DATA_02	0x4B
#define MPU6050_EXT_SENS_DATA_03	0x4C
#define MPU6050_EXT_SENS_DATA_04	0x4D
#define MPU6050_EXT_SENS_DATA_05	0x4E
#define MPU6050_EXT_SENS_DATA_06	0x4F
#define MPU6050_EXT_SENS_DATA_07	0x50
#define MPU6050_EXT_SENS_DATA_08	0x51
#define MPU6050_EXT_SENS_DATA_09	0x52
#define MPU6050_EXT_SENS_DATA_10	0x53
#define MPU6050_EXT_SENS_DATA_11	0x54
#define MPU6050_EXT_SENS_DATA_12	0x55
#define MPU6050_EXT_SENS_DATA_13	0x56
#define MPU6050_EXT_SENS_DATA_14	0x57
#define MPU6050_EXT_SENS_DATA_15	0x58
#define MPU6050_EXT_SENS_DATA_16	0x59
#define MPU6050_EXT_SENS_DATA_17	0x5A
#define MPU6050_EXT_SENS_DATA_18	0x5B
#define MPU6050_EXT_SENS_DATA_19	0x5C
#define MPU6050_EXT_SENS_DATA_20	0x5D
#define MPU6050_EXT_SENS_DATA_21	0x5E
#define MPU6050_EXT_SENS_DATA_22	0x5F
#define MPU6050_EXT_SENS_DATA_23	0x60
#define MPU6050_MOT_DETECT_STATUS	0x61
#define MPU6050_MOT_XNEG					0X80
#define MPU6050_MOT_XPOS					0X40
#define MPU6050_MOT_YNEG					0X20
#define MPU6050_MOT_YPOS					0X10
#define MPU6050_MOT_ZNEG					0X08
#define MPU6050_MOT_ZPOS					0X04
#define MPU6050_MOT_ZROMT					0X01


#define MPU6050_I2C_SLV0_DO			0x63
#define MPU6050_I2C_SLV1_DO			0x64
#define MPU6050_I2C_SLV2_DO			0x65
#define MPU6050_I2C_SLV3_DO			0x66
#define MPU6050_I2C_MST_DELAY_CTRL	0x67
#define MPU6050_DELAY_ES_SHADOW				0X80
#define MPU6050_I2C_SLV4_DLY_EN				0X10
#define MPU6050_I2C_SLV3_DLY_EN				0X08
#define MPU6050_I2C_SLV2_DLY_EN				0X04
#define MPU6050_I2C_SLV1_DLY_EN				0X02
#define MPU6050_I2C_SLV0_DLY_EN				0X01



#define MPU6050_SIGNAL_PATH_RESET 	0x68
#define MPU6050_GYRO_RESET  				0X04
#define MPU6050_ACCEL_RESET 				0X02
#define MPU6050_TEMP_RESET  				0X01

#define MPU6050_MOT_DETECT_CTRL   	0x69
#define MPU6050_ACCEL_ON_DELAY_BIT1			0X20
#define MPU6050_ACCEL_ON_DELAY_BIT0			0X10
#define MPU6050_FF_COUNT_BIT1				0X08
#define MPU6050_FF_COUNT_BIT0				0X04
#define MPU6050_MOT_COUNT_BIT1				0X02
#define MPU6050_MOT_COUNT_BIT0				0X01

#define MPU6050_USER_CTRL       	0x6A
#define MPU6050_DMP_EN_BIT	             		0x80
#define MPU6050_FIFO_EN_BIT 					0X40
#define MPU6050_I2C_MST_EN_BIT					0X20
#define MPU6050_I2C_IF_DIS_BIT					0X10
#define MPU6050_DMP_RESET_BIT					0x08
#define MPU6050_FIFO_RESET_BIT					0X04
#define MPU6050_I2C_MST_RESET_BIT				0X02
#define MPU6050_I2C_COND_RESET_BIT				0X01

#define MPU6050_PWR_MGMT_1      	0x6B
#define MPU6050_DEVICE_RESET				0X80
#define MPU6050_SLEEP 						0X40
#define MPU6050_CYCLE 						0X20
#define MPU6050_TEMP_DIS					0X08
#define MPU6050_Internal_8MHz_oscillator	0x00
#define MPU6050_PLL_with_X_axis_gyroscope_reference 0x01
#define MPU6050_PLL_with_Y_axis_gyroscope_reference	0X02
#define MPU6050_PLL_with_Z_axis_gyroscope_reference 0X03
#define MPU6050_PLL_with_external_32_768kHz_reference 0X04
#define MPU6050_PLL_with_external_19_2MHz_reference 0x05
#define MPU6050_Stops_the_clock_and_keeps_the_timing_generator_in_reset 0x07
#define MPU6050_CLKSEL_BIT2					0X04
#define MPU6050_CLKSEL_BIT1					0X02
#define MPU6050_CLKSEL_BIT0					0X01

#define MPU6050_PWR_MGMT_2      	0x6C
#define MPU6050_LP_WAKE_CTRL_BIT1			0X80
#define MPU6050_LP_WAKE_CTRL_BIT0			0X40
#define MPU6050_STBY_XA						0X20
#define MPU6050_STBY_YA						0X10
#define MPU6050_STBY_ZA						0X08
#define MPU6050_STBY_XG						0X04
#define MPU6050_STBY_YG						0X02
#define MPU6050_STBY_ZG						0X01

#define MPU6050_BANK_SEL        	0x6D
#define MPU6050_MEM_START_ADDR  	0x6E
#define MPU6050_MEM_R_W         	0x6F
#define MPU6050_DMP_CFG_1       	0x70
#define MPU6050_DMP_CFG_2       	0x71
#define MPU6050_FIFO_COUNTH     	0x72
#define MPU6050_FIFO_COUNTL     	0x73
#define MPU6050_FIFO_R_W        	0x74
#define MPU6050_WHO_AM_I        	0x75




void Single_WriteMPU6050(uint8_t REG,uint8_t DATA);
uint8_t Single_ReadMPU6050(uint8_t REG);
//int Burst_ReadMPU6050(uint8_t REG);
void InitMPU6050(void);
//void intDMP(void);
//void readdmp(void);

void loadfirmware(void);
void readdmp(void);
void readFIFO(void);

long getdmplong(uint8_t address);
void getquaternion(void);
void getgyro(void);
void getaccel(void);
char ComputePID(int DTms, int DTinv, int in, int setPoint, int *errorSum,int *errorOld,float Kp,  float Ki,  float Kd);


//long getdmplong(uint8_t address);
//void getquaternion(void);
void getyawpitchroll(void);
uint8_t getroll(void);
uint16_t getpitch(void);
uint16_t getyaw(void);
//void DMPStart(void);
//void displayDMP(void);
#ifdef __cplusplus
}
#endif
#endif
// 		LCD_Num(0,1,Single_ReadMPU6050(RA_X_FINE_GAIN));//180
// 		LCD_Num(0,2,Single_ReadMPU6050(RA_Y_FINE_GAIN));//221
// 		LCD_Num(0,3,Single_ReadMPU6050(RA_Z_FINE_GAIN));//192
// 		LCD_Num(0,4,Single_ReadMPU6050(RA_XG_OFFS_TC));//131
// 		LCD_Num(0,5,Single_ReadMPU6050(RA_YG_OFFS_TC));//125
// 		LCD_Num(0,6,Single_ReadMPU6050(RA_ZG_OFFS_TC));//255
// 		LCD_Num(0,7,(RA_XA_OFFS_H<<8)+RA_XA_OFFS_L_TC);//1543
// 		LCD_Num(84,0,(RA_YA_OFFS_H<<8)+RA_YA_OFFS_L_TC);//2057
// 		LCD_Num(84,1,(RA_ZA_OFFS_H<<8)+RA_ZA_OFFS_L_TC);//2571
// 		LCD_Num(84,2,(RA_XG_OFFS_USRH<<8)+RA_XG_OFFS_USRL);//4884
// 		LCD_Num(84,3,(RA_YG_OFFS_USRH<<8)+RA_YG_OFFS_USRL);//5398
// 		LCD_Num(84,4,(RA_ZG_OFFS_USRH<<8)+RA_ZG_OFFS_USRL);//5912
// 		LCD_Num(84,5,AUX_VDDIO);